using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;
using surfInterop;
using DotNetMatrix;


namespace Blimpbots.Recognition
{
    [Serializable]
    public struct Point2D
    {
        /// <summary>
        /// A utility function which creates a homogeneous matrix out of the given point.
        /// </summary>
        public static GeneralMatrix ToMatrix(double x, double y)
        {
            return new GeneralMatrix(new double[] { x, y, 1.0 }, 3);
        }

        public double x; public double y;
        public Point2D(double x, double y)
        { this.x = x; this.y = y; }

        public double Norm()
        {
            return Math.Sqrt(this.x * this.x + this.y * this.y);
        }

        public double Dot(Point2D b)
        {
            return (this.x * b.x) + (this.y * b.y); 
        }

        public double Distance(Point2D point)
        {
            return Math.Sqrt(Math.Pow(this.x - point.x, 2) 
                 + Math.Pow(this.y - point.y, 2));
        }

        public System.Drawing.PointF ToPointF()
        {
            return new System.Drawing.PointF((float)this.x, (float)this.y);
        }
        public override string ToString() {
            return string.Format("[{0:f2},{1:f2}]", x, y);
        }
    }

    public class KeypointMatch
    {
        public int modelIdx; public int dataIdx;
        public Point2D modelCoord; public Point2D dataCoord;
        public int distance; public double ratio;
        public double beta; // data - model ori
        public double modelScale; public double dataScale;

        public KeypointMatch(int modelIdx, int dataIdx, Point2D modelCoord, 
                             Point2D dataCoord, int distance, double ratio, 
                             double modelScale, double dataScale, double beta) 
        {
            this.modelIdx = modelIdx; this.dataIdx = dataIdx; 
            this.modelCoord = modelCoord; this.dataCoord = dataCoord;
            this.distance = distance; // featurespace distance
            this.ratio = ratio;
            this.modelScale = modelScale;
            this.dataScale = dataScale;
            this.beta = beta;
        }
    }


    public class Matching
    {
        public enum MatchStatus
        {
            Unknown,
            Matched,
            NotMatched
        }

        private IList<SurfKeypoint> _model;
        private double[] _maxAllowedError;
        private int[] _keypointFrequency;
        private bool useMaxAllowedError = false;

        public Matching(IList<SurfKeypoint> model)
        {
            _model = model;
        }

        public struct ModelBasedKeypoint
        {
            public SurfKeypoint k;
            public int modelIdx;
            public double maxAllowedError;
            public ModelBasedKeypoint(SurfKeypoint k, int modelIdx, double expectedDistance)
            { this.k = k; this.modelIdx = modelIdx; this.maxAllowedError = expectedDistance; }
        }

        /// <summary>
        /// Constructs a matcher using knowledge about keypoint relevance
        /// and usual distance.
        /// </summary>
        /// <param name="useModelLearnedError">Specifies whether to allow matches with similar distance to that observed in model Self-Correlation.  This is useful for models with many identical features, but slows processing time.</param>
        public Matching(IList<SurfKeypoint> model,
            int[] keypointStrength, double[] maxKeypointError,
            bool useModelLearnedError)
        {
            _model = model;
            _keypointFrequency = keypointStrength;
            _maxAllowedError = maxKeypointError;
            useMaxAllowedError = useModelLearnedError;
        }

        /// <summary>
        /// Returns a list of the top match possibilities.  It will produce many false positives, but should
        /// include the full list of actual matches.  Good when we know how many matches *should* exist, such
        /// as during model building.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="topPercent"></param>
        /// <returns></returns>
        public List<KeypointMatch> MatchZealous(IList<SurfKeypoint> data, int topN)
        {
            if (topN > (data.Count * _model.Count))
            {
                topN = data.Count * _model.Count;
                //throw new Exception("Cannot return more matches than (data*model) size.");
            }

            List<KeypointMatch> matches = new List<KeypointMatch>(data.Count*_model.Count);

            for(int i = 0; i < data.Count; i++)
                for(int j = 0; j < _model.Count; j++)
                {
                    int dist = BoundedDistance(data[i], _model[j], int.MaxValue);
                    matches.Add(new KeypointMatch(
                        j, i, 
                        new Point2D(_model[j].x, _model[j].y),
                        new Point2D(data[i].x, data[i].y), dist, 1.0,
                        _model[j].scale, data[i].scale,
                        data[i].orientation - _model[j].orientation
                        ));
                }

            matches.Sort(
                delegate(KeypointMatch m1, KeypointMatch m2)
                { return m1.distance.CompareTo(m2.distance); });

            List<KeypointMatch> result = new List<KeypointMatch>(topN);
           
            for (int i = 0; i < topN; i++)
                result.Add(matches[i]);

            return result;
        }

        private MatchStatus[] _matchStatus = null;
        private MatchStatus[] _matchStatusData = null; 

        public void SetMatchStatus(MatchStatus[] modelStatus, MatchStatus[] dataStatus)
        {
            _matchStatus = modelStatus; _matchStatusData = dataStatus;
        }

        public List<KeypointMatch> Match1(IList<SurfKeypoint> data, double ratioThresh)
        {
            if (_maxAllowedError == null) useMaxAllowedError = false;

            return MatchSecondNN(data, ratioThresh);
        }


        public List<KeypointMatch> Match(IList<SurfKeypoint> data, double ratioThresh)
        {
            if (_maxAllowedError == null) useMaxAllowedError = false;

            //return MatchBBF(data, ratioThresh);

            // If we don't have knowledge on the expected distances,
            // fall back to the nearest & second nearest neighbor algorithm
            return MatchSecondNN2(data, ratioThresh);


        }


        private List<KeypointMatch> MatchSecondNN2(IList<SurfKeypoint> data, double ratioThresh)
        {
            // 2 x model features should be a good size.
            List<KeypointMatch> matches = new List<KeypointMatch>(_model.Count * 2);

            for (int j = 0; j < _model.Count; j++)
            {
                // Skip items that already matched or are known to not match.
                if (_matchStatus != null && _matchStatus[j] != MatchStatus.Unknown)
                    continue;
                

                List<KeypointMatch> neighbors = new List<KeypointMatch>();

                int nearest = int.MaxValue;
                int secondNearest = int.MaxValue;

                int nearestIdx = -1;
                int secondNearestIdx = -1;

                for (int i = 0; i < data.Count; i++)
                {
                    // Skip keypoints which have already been matched to some model point
                    if (_matchStatusData != null && _matchStatusData[i] != MatchStatus.Unknown)
                        continue;

                    int dist = BoundedDistance(_model[j], data[i], secondNearest);

                    // Keep track of all matches within the tolerance
                    if (dist < secondNearest)
                    {
                        neighbors.Add(new KeypointMatch(j, i,
                            new Point2D(_model[j].x, _model[j].y),
                            new Point2D(data[i].x, data[i].y), dist, dist / secondNearest,
                            _model[j].scale, data[i].scale,
                            data[i].orientation - _model[j].orientation
                            ));
                    }

                    #region Update nearest and 2nd nearest
                    if (dist < nearest)
                    {
                        secondNearest = nearest; secondNearestIdx = nearestIdx;
                        nearest = dist; nearestIdx = i;
                    }
                    else if (dist < secondNearest)
                    {
                        secondNearest = dist;
                        secondNearestIdx = i;
                    }
                    #endregion
                }

                // Second nearest-neighbor ratio method
                if (nearest < ratioThresh * secondNearest)
                {
                    // Ensure that the index was changed from -1
                    if (nearestIdx >= 0 && secondNearestIdx >= 0)
                    {
                        matches.Add(new KeypointMatch(j, nearestIdx,
                            new Point2D(_model[j].x, _model[j].y),
                            new Point2D(data[nearestIdx].x, data[nearestIdx].y), nearest, nearest / secondNearest,
                            _model[j].scale, data[nearestIdx].scale, 
                            data[nearestIdx].orientation - _model[j].orientation
                            ));
                    }
                }

            }

            return matches;
        }


        private List<KeypointMatch> MatchSecondNN(IList<SurfKeypoint> data, double ratioThresh)
        {
            // 2 x model features should be a good size.
            List<KeypointMatch> matches = new List<KeypointMatch>(_model.Count * 2);

            for(int i = 0; i < data.Count; i++)
            {
                // Skip keypoints which have already been matched to some model point
                if (_matchStatusData != null && _matchStatusData[i] != MatchStatus.Unknown)
                    continue;

                List<KeypointMatch> neighbors = new List<KeypointMatch>();

                int nearest = int.MaxValue;
                int secondNearest = int.MaxValue;

                int nearestIdx = -1;
                int secondNearestIdx = -1;

                for (int j = 0; j < _model.Count; j++)
                {
                    // Skip items that already matched or are known to not match.
                    if (_matchStatus != null && _matchStatus[j] != MatchStatus.Unknown)
                        continue;

                    int dist = BoundedDistance(_model[j], data[i], secondNearest);

                    // Keep track of all matches within the tolerance
                    if (dist < secondNearest)
                    {
                        neighbors.Add(new KeypointMatch(j, i,
                            new Point2D(_model[j].x, _model[j].y),
                            new Point2D(data[i].x, data[i].y), dist, dist / secondNearest,
                            _model[j].scale, data[i].scale, 
                            data[i].orientation - _model[j].orientation
                            ));
                    }

                    #region Update nearest and 2nd nearest
                    if (dist < nearest)
                    {
                        secondNearest = nearest; secondNearestIdx = nearestIdx;
                        nearest = dist; nearestIdx = j;
                    }
                    else if (dist < secondNearest)
                    {
                        secondNearest = dist;
                        secondNearestIdx = j;
                    }
                    #endregion
                }

                // Second nearest-neighbor ratio method
                if (nearest < ratioThresh * secondNearest)
                {
                    // Ensure that the index was changed from -1
                    if (nearestIdx >= 0 && secondNearestIdx >= 0)
                    {
                        matches.Add(new KeypointMatch(nearestIdx, i,
                            new Point2D(_model[nearestIdx].x, _model[nearestIdx].y),
                            new Point2D(data[i].x, data[i].y), nearest, nearest / secondNearest,
                            _model[nearestIdx].scale, data[i].scale, 
                            data[i].orientation - _model[nearestIdx].orientation
                            ));
                    }
                }

            }

            return matches;
        }

        public int BoundedDistance(SurfKeypoint a, SurfKeypoint b, int bound)
        {
            int dist = 0;

            for (int i = 0; i < a.i_descriptor.Length; i++)
            {
                int d = (a.i_descriptor[i] - b.i_descriptor[i]);
                dist += d * d;
                if (dist > bound) break; // short-circuit computation
            }

            return dist;
        }
 




    }
}
